OBJ

DEBUG      : "SerialMirror"  
GDB        : "GravityDevelopmentBoardDriver"
CLK        : "Clock"

CON
'Declare valve ON FLAG enumerations. 
#30, ValvePitchDown , ValvePitchUp, ValveRollClock, ValveRollCClock, ValveYawLeft, ValveYawRight

'Declare finger enumerations. Correlates to hardware Parallax Pins used
#0, R_Palm, R_Pinky, R_RingF, R_MiddleF, R_IndexF, R_Thumb
#6, L_Palm, L_Pinky, L_RingF, L_MiddleF, L_IndexF, L_Thumb 
VAR

'' VARIABLE CONSTANTS defined in Initialize function
'' Saves memory by using byte, instead of long for each CONSTANT 
byte DEBUG_MODE
byte OPEN
byte CLOSE
byte RETRO
byte USER
  
byte valveFlags 'Two Least Significcant Bits (LSB) are not used

long pitchDownTimeBase
long pitchUpTimeBase
long rollClockTimeBase
long rollCClockTimeBase   
long yawLeftTimeBase  
long yawRightTimeBase 
   

'Retrograde (RG) time remaining  
'long RG_Times[6]
long RG_PitchDownTime   'Time left for PitchDown thurster firing 
long RG_PitchUpTime 
long RG_RollClockTime
long RG_RollCClockTime
long RG_RYawLeftTime
long RG_RYawRightTime

PUB Initialize

'' "VARIABLE" CONSTANTS defined  
DEBUG_MODE := true 'DEBUG#DEBUG_STATE
OPEN := 1
CLOSE := 2
RETRO := 11
USER := 22 
valveFlags := %0000_0000

DEBUG.Str(STRING("Zex Pro Fuel Solenoid Driver Initialization", DEBUG#CR))

if(DEBUG_MODE)
  DEBUG.Str(STRING("Zex Pro Fuel Solenoid Driver Debug function is ON", DEBUG#CR)) 

return

PUB OpenPitchDownValve(RetrogradeMultiplier)
''mode - User input function call or retrograde function call
''Connect Valve to Channel 2 of L298 U1 on Gravity Development Board

'if(DEBUG_MODE)
DEBUG.Str(STRING("Opening Pitch Down Valve", GDB#CR))
  
if( CheckValveFlag(ValvePitchUp) )       'If valve 2 is open (probably for the Retro-thrust)
  ClosePitchUpValve(RetrogradeMultiplier)                       'Close it
  ClearValveFlag(ValvePitchUp)            'Set v2 flag to false


pitchDownTimeBase := cnt
DEBUG.Str(STRING(GDB#CR))
DEBUG.Str(STRING("Start time = ", GDB#CR))                                   'Set a start time
DEBUG.Dec(pitchDownTimeBase)
'CLK.PauseMSec(1500)  'DEBUG STATEMENT
GDB.FullH_BridgeU1Subroutine(2, OPEN, GDB#INFINITY)     'Open valve 1  ??? Pitch Down Valve
RaiseValveFlag(ValvePitchDown)                          'Set v1 flag to true

return  


PUB ClosePitchDownValve(RetrogradeMultiplier)
''Connect Valve to Channel 2 of L298 U1 on Gravity Development Board

DEBUG.Str(STRING("Closing Pitch Down Valve ", GDB#CR)) 

GDB.FullH_BridgeU1Subroutine(2, CLOSE, GDB#INFINITY)    'Close valve 1  
ClearValveFlag(ValvePitchDown)                          'Set v1 flag to false


  RG_PitchUpTime := cnt - pitchDownTimeBase             'Set retrograde time
  if(RG_PitchUpTime <= 0)
    DEBUG.Str(STRING("COUNTER ROLL OVER OCCURED :( ", GDB#CR))
    RG_PitchUpTime := RG_PitchUpTime + $FFFF_FFFF

  DEBUG.Str(STRING("RG Pitch Up Time = ", GDB#CR))  
  DEBUG.Dec(RG_PitchUpTime)
'  CLK.PauseMSec(1500)  'DEBUG STATEMENT

  FireRGPitchUpThruster(RetrogradeMultiplier)                                   'Run RG method (thus opening valve 2)

'DO NOTHING ELSE
return

PUB OpenPitchUpValve(RetrogradeMultiplier)
''Connect Valve to Channel 1 of L298 U1 on Gravity Development Board
''mode - User input function call or retrograde function call

'if(DEBUG_MODE)
DEBUG.Str(STRING("Opening Pitch Up Valve", GDB#CR))
  
if( CheckValveFlag(ValvePitchDown) )       'If valve 1 is open (probably for the Retro-thrust)
  ClosePitchDownValve(RetrogradeMultiplier)                       'Close it
  ClearValveFlag(ValvePitchDown)            'Set v1 flag to false


pitchUpTimeBase := cnt
DEBUG.Str(STRING(GDB#CR))
DEBUG.Str(STRING("Opening Pitch Up Valve - Start time = ", GDB#CR))                                 'Set a start time
DEBUG.Dec(pitchUpTimeBase)
'CLK.PauseMSec(1500)  'DEBUG STATEMENT
GDB.FullH_BridgeU1Subroutine(1, OPEN, GDB#INFINITY)     'Open valve 1  ??? Pitch Down Valve
RaiseValveFlag(ValvePitchUp)                          'Set v1 flag to true

return  


PUB ClosePitchUpValve(RetrogradeMultiplier)
''Connect Valve to Channel 1 of L298 U1 on Gravity Development Board

DEBUG.Str(STRING("Closing Pitch Up Valve ", GDB#CR)) 

GDB.FullH_BridgeU1Subroutine(1, CLOSE, GDB#INFINITY)    'Close valve 2  
ClearValveFlag(ValvePitchUp)                          'Set v2 flag to false


RG_PitchDownTime := cnt - pitchUpTimeBase             'Set retrograde time
if(RG_PitchDownTime <= 0)
  DEBUG.Str(STRING("COUNTER ROLL OVER OCCURED :( ", GDB#CR))
  RG_PitchDownTime := RG_PitchDownTime + $FFFF_FFFF

DEBUG.Str(STRING("RG Pitch Down Time = ", GDB#CR))  
DEBUG.Dec(RG_PitchDownTime)
'CLK.PauseMSec(1500)  'DEBUG STATEMENT

FireRGPitchDownThruster(RetrogradeMultiplier)                                   'Run RG method (thus opening valve 2)

return

PRI FireRGPitchUpThruster(RetrogradeMultiplier) | localStartTime

DEBUG.Str(STRING("Firing Pitch Up thruster in Retrograde mode", GDB#CR))
  
localStartTime := cnt                              'Sets the start time of the retrograde thrust
repeat while(((cnt - localStartTime) * RetrogradeMultiplier) < RG_PitchDownTime)  'Loops until the RG_Time is fulfilled
   OpenPitchUpValve(RetrogradeMultiplier) 
                                  'Open valve 2
   RaiseValveFlag(ValvePitchUp)                   'Set v2 flag to true
   if ((INA[R_IndexF] & INA[R_Thumb]) | (INA[R_MiddleF] & INA[R_Thumb])) 'If a finger is pressed, while the RG thrusting...
     QUIT                                                              ' EXIT LOOP
                                                  
ClosePitchUpValve(RetrogradeMultiplier)                                  'Close valve 2
ClearValveFlag(ValvePitchUp)                      'Set v2 flag to false
RG_PitchDownTime -= (cnt - localStartTime)

                              
PRI FireRGPitchDownThruster(RetrogradeMultiplier) | localStartTime
if(DEBUG_MODE)
  DEBUG.Str(STRING("Firing Pitch Down thruster in Retrograde mode", GDB#CR))

localStartTime := cnt                              'Sets the start time of the retrograde thrust
repeat while(((cnt - localStartTime) * RetrogradeMultiplier) < RG_PitchUpTime)  'Loops until the RG_Time is fulfilled
   OpenPitchUpValve(RetrogradeMultiplier)
                                  'Open valve 2
   RaiseValveFlag(ValvePitchUp)                   'Set v2 flag to true
   if ((INA[R_IndexF] & INA[R_Thumb]) | (INA[R_MiddleF] & INA[R_Thumb])) 'If a finger is pressed, while the RG thrusting...
     QUIT                                                              ' EXIT LOOP
                                                  
ClosePitchUpValve(RetrogradeMultiplier)                                 'Close valve 2
ClearValveFlag(ValvePitchUp)                      'Set v2 flag to false
RG_PitchDownTime -= (cnt - localStartTime)

PUB GetRG_PitchDownTime :RG_PDT
RG_PDT := RG_PitchDownTime
return RG_PDT

PUB GetRG_PitchUpTime :RG_PUT
RG_PUT := RG_PitchUpTime
return RG_PUT

PUB CheckValveFlag(FlagType) | flag

if(DEBUG_MODE)
  DEBUG.Str(STRING("Checking Valve flags", GDB#CR))
  
case FlagType
  valvePitchDown :
    flag := valveFlags & %1000_0000
  valvePitchUp :
    flag := valveFlags & %0100_0000  
  valveRollClock :
    flag := valveFlags & %0010_0000
  valveRollCClock :
    flag := valveFlags & %0001_0000
  valveYawLeft :
    flag := valveFlags & %0000_1000
  valveYawRight :
    flag := valveFlags & %0000_0100  

if(DEBUG_MODE)
  DEBUG.Str(STRING("Flag = "))
  DEBUG.Dec(flag)

return flag 'Non-zero flag equals TRUE, otherwise FALSE 

PUB RaiseValveFlag(FlagType)

if(DEBUG_MODE)
  DEBUG.Str(STRING("Rasising Valve flags", GDB#CR))
  
case FlagType
  valvePitchDown :
   valveFlags |= %1000_0000
  valvePitchUp :
    valveFlags |= %0100_0000  
  valveRollClock :
    valveFlags |= %0010_0000 
  valveRollCClock :
    valveFlags |= %0001_0000 
  valveYawLeft :
    valveFlags |= %0000_1000 
  valveYawRight :
    valveFlags |= %0000_0100  

if(DEBUG_MODE)
  DEBUG.Str(STRING("Flag Status = "))
  DEBUG.Dec(valveFlags)

PUB ClearValveFlag(FlagType)

if(DEBUG_MODE)
  DEBUG.Str(STRING("Clearing Valve flags", GDB#CR))
  
case FlagType
  valvePitchDown :
   valveFlags &= %0111_1111
  valvePitchUp :
    valveFlags &= %1011_1111   
  valveRollClock :
    valveFlags &= %1101_1111 
  valveRollCClock :
    valveFlags &= %1110_1111
  valveYawLeft :
    valveFlags &= %1111_0111
  valveYawRight :
    valveFlags &= %1111_1011  

if(DEBUG_MODE)
  DEBUG.Str(STRING("Flag Status = "))
  DEBUG.Dec(valveFlags)

PUB UnitTest
'Test each valve

   